#include "main.h"
#include "spi.h"
#include "DriversStruct.h"
#include "BMI088.h"

/*  SPI1硬件 模式0，        软件控制片选
 *  CS1是加速度计片选       CS2是陀螺仪片选
 *  INT1是加速度计中断引脚  INT3是陀螺仪中断引脚
 *  片选引脚拉低 对应设备启动
 */

#define SPI1_ACC_Enable                                            \
    {                                                              \
        HAL_GPIO_WritePin(CS1_GPIO_Port, CS1_Pin, GPIO_PIN_RESET); \
        HAL_GPIO_WritePin(CS2_GPIO_Port, CS2_Pin, GPIO_PIN_SET);   \
    }
#define SPI1_ACC_Disable                                         \
    {                                                            \
        HAL_GPIO_WritePin(CS1_GPIO_Port, CS1_Pin, GPIO_PIN_SET); \
        HAL_GPIO_WritePin(CS2_GPIO_Port, CS2_Pin, GPIO_PIN_SET); \
    }
#define SPI1_GYRO_Enable                                           \
    {                                                              \
        HAL_GPIO_WritePin(CS1_GPIO_Port, CS1_Pin, GPIO_PIN_SET);   \
        HAL_GPIO_WritePin(CS2_GPIO_Port, CS2_Pin, GPIO_PIN_RESET); \
    }
#define SPI1_GYRO_Disable                                        \
    {                                                            \
        HAL_GPIO_WritePin(CS1_GPIO_Port, CS1_Pin, GPIO_PIN_SET); \
        HAL_GPIO_WritePin(CS2_GPIO_Port, CS2_Pin, GPIO_PIN_SET); \
    }

/*用于选择是陀螺仪还是加速度计*/
#define CS_GYRO 0
#define CS_ACC 1

static uint8_t BMI088_ACCData[6];
static uint8_t BMI088_GYROData[6];

/**
 * @brief 用于BMI088传感器中的软件延时
 *
 * @param times
 */
static void BMI088_Delay(int times)
{
    HAL_Delay(times);
}

/**
 * @brief 通过SPI通讯总线读取和发送传感器数据
 *
 * @param data:发送给传感器的数据
 * @return uint8_t
 */
static uint8_t BMI088_SPIReadSend(uint8_t data)
{
    uint8_t ret = 0xff;
    HAL_SPI_TransmitReceive(&hspi1, &data, &ret, 1, 0xffff);
    return ret;
}

/**
 * @brief 读取传感器寄存器数据
 *
 * @param Addr:寄存器地址
 * @param ACC_OR_GYRO:选择加速度计或陀螺仪
 * @return uint8_t
 */
static uint8_t BMI088_Read_Reg(uint8_t Addr, uint8_t ACC_OR_GYRO)
{
    uint8_t val = 0;
    if (ACC_OR_GYRO == CS_ACC)
    {
        SPI1_ACC_Enable;
        BMI088_SPIReadSend(Addr | 0x80);
        BMI088_SPIReadSend(0x00); // 这次的数据作废（在数据手册第6章提到过）
        val = (uint8_t)(BMI088_SPIReadSend(0x00) & 0xFF);
        SPI1_ACC_Disable;
    }
    else if (ACC_OR_GYRO == CS_GYRO)
    {
        SPI1_GYRO_Enable;
        BMI088_SPIReadSend(Addr | 0x80); // 最高位置1 表示读操作  （问题1）
        val = (uint8_t)(BMI088_SPIReadSend(0x00) & 0xFF);
        SPI1_GYRO_Disable;
    }
    return val;
}

/**
 * @brief 写传感器寄存器数据
 *
 * @param Addr:寄存器地址
 * @param Val:值
 * @param ACC_OR_GYRO:选择加速度计或陀螺仪
 */
static void BMI088_Write_Reg(uint8_t Addr, uint8_t Val, uint8_t ACC_OR_GYRO)
{
    if (ACC_OR_GYRO == CS_ACC)
    {
        SPI1_ACC_Enable;
    }
    else if (ACC_OR_GYRO == CS_GYRO)
    {
        SPI1_GYRO_Enable;
    }

    BMI088_SPIReadSend(Addr & 0x7f); // 0x7F 0111 1111  相当于最高位置0 写操作（问题1）
    BMI088_SPIReadSend(Val);         // 写数据

    if (ACC_OR_GYRO == CS_ACC)
    {
        SPI1_ACC_Disable;
    }
    else if (ACC_OR_GYRO == CS_GYRO)
    {
        SPI1_GYRO_Disable;
    }
}

/**
 * @brief   初始化加速度计
 *
 * @note:	配置加速度计产生中断的IO口的输出方式，数据输出频率1600hz
                加速度量程是 ±24g 有点大，目前的车无需这么大的量程
 */
int ret = 0;
uint8_t acc_id;
static uint8_t BMI088_ACC_Congfig(void)
{
    SPI1_ACC_Disable;                     
    BMI088_Write_Reg(0x7e, 0xb6, CS_ACC); // 软复位    详见手册第5章 0X7E指令
    while (acc_id != ACC_CHIP_ID)         // 等待ID为加速度计
    {
        BMI088_Delay(10);
        acc_id = BMI088_Read_Reg(0x00, CS_ACC); // id:1E
    }
    BMI088_Delay(5); //> 1 ms ;
    ret = 0;
    while (ret != ACC_ON) // 等待开启
    {
        BMI088_Write_Reg(ACC_PWR_CTRL, ACC_ON, CS_ACC); // 写开启
        ret = BMI088_Read_Reg(ACC_PWR_CTRL, CS_ACC);
        BMI088_Delay(5);
    }
    /* 可根据实际使用和加速度寄存器表修改加速度计测量范围 */
    while (BMI088_Read_Reg(ACC_RANG, CS_ACC) != Plus_Minus_6G)
    {
        BMI088_Write_Reg(ACC_RANG, Plus_Minus_6G, CS_ACC); // 目前6G
        BMI088_Delay(5);
    }
    while (BMI088_Read_Reg(0x40, CS_ACC) != 0xBC)
    {
        /* 可根据实际使用和加速度寄存器表修改加速度计数据读取频率 */
        BMI088_Write_Reg(0x40, 0xBC, CS_ACC); //  1600HZ
        BMI088_Delay(5);
    }
    while (BMI088_Read_Reg(0X53, CS_ACC) != 0X08)
    {
        BMI088_Write_Reg(0x53, 0x08, CS_ACC); // 0000 1000 INT1设置推挽输出
        BMI088_Delay(5);
    }
    while (BMI088_Read_Reg(0x58, CS_ACC) != 0x04)
    {
        BMI088_Write_Reg(0x58, 0x04, CS_ACC); // 0000 0100 INT1开启中断
        BMI088_Delay(5);
    }
    BMI088_Delay(20);
    while (BMI088_Read_Reg(ACC_PWR_CONF, CS_ACC) != ACC_ACTIVE) // 开启ACC正常模式
    {
        BMI088_Write_Reg(ACC_PWR_CONF, ACC_ACTIVE, CS_ACC); // ACtive mode
        BMI088_Delay(600);                                  //>50ms
    }
    return 0;
}

/**
 * @brief 初始化陀螺仪
 *
 * @note:配置陀螺仪产生中断的IO口的输出方式，数据输出频率2000hz
 */
uint8_t gyro_id;
static uint8_t BMI088_GYRO_Congfig(void)
{
    SPI1_GYRO_Disable; // 关闭片选

    while (gyro_id != GYRO_CHIP_ID) // 读取ID
    {
        gyro_id = BMI088_Read_Reg(0x00, CS_GYRO); // id:0x0f
        BMI088_Delay(5);
    }
    while (BMI088_Read_Reg(GYRO_RANG, CS_GYRO) != Plus_Minus_500)
    {
        BMI088_Write_Reg(GYRO_RANG, Plus_Minus_500, CS_GYRO); // rang +-500 °/s
        BMI088_Delay(5);
    }

    /* 可根据陀螺仪寄存器对照表陀螺仪采样率是2000hz */
    BMI088_Write_Reg(GYRO_BANDWIDTH, ODR_2000_FD_230, CS_GYRO);

    while (BMI088_Read_Reg(0X11, CS_GYRO) != 0x00)
    {
        BMI088_Write_Reg(0X11, 0x00, CS_GYRO); // 电源正常模式
        BMI088_Delay(5);
    }
    while (BMI088_Read_Reg(0X15, CS_GYRO) != 0X80)
    {
        BMI088_Write_Reg(0X15, 0X80, CS_GYRO); // 设置中断
        BMI088_Delay(5);
    }
    while (BMI088_Read_Reg(0X16, CS_GYRO) != 0X00)
    {
        BMI088_Write_Reg(0X16, 0X00, CS_GYRO); // 配置INT3、INT4活动状态
        BMI088_Delay(5);
    }
    while (BMI088_Read_Reg(0X18, CS_GYRO) != 0X01)
    {
        BMI088_Write_Reg(0X18, 0X01, CS_GYRO); // 中断映射到INT3引脚
        BMI088_Delay(5);
    }
    return 0;
}

/**
 * @brief 传感器初始化，置UpdataFlag为0
 *
 * @return uint8_t 初始化成功返回1
 */
uint8_t BMI088_Init(void)
{
    BMI088_ACC_Congfig();
    BMI088_GYRO_Congfig();
    DebugValue.BMI088_OK = 1;
    return 1;
}

/**
 * @brief 尝试更新加速度计数据，成功返回1，失败返回0
 *
 * @return
 */
void BMI088_UpdataACCData(void)
{

    BMI088_ACCData[0] = BMI088_Read_Reg(0X12, CS_ACC); // X_LSB
    BMI088_ACCData[1] = BMI088_Read_Reg(0X13, CS_ACC); // X_MSB
    BMI088_ACCData[2] = BMI088_Read_Reg(0X14, CS_ACC); // Y_LSB
    BMI088_ACCData[3] = BMI088_Read_Reg(0X15, CS_ACC); // Y_MSB
    BMI088_ACCData[4] = BMI088_Read_Reg(0X16, CS_ACC); // Z_LSB
    BMI088_ACCData[5] = BMI088_Read_Reg(0X17, CS_ACC); // Z_MSB
}

/**
 * @brief 尝试更新陀螺仪数据，成功返回1，失败返回0
 *
 * @return
 */
void BMI088_UpdataGYROData(void)
{

    BMI088_GYROData[0] = BMI088_Read_Reg(0X02, CS_GYRO); // X_LSB
    BMI088_GYROData[1] = BMI088_Read_Reg(0X03, CS_GYRO); // X_MSB
    BMI088_GYROData[2] = BMI088_Read_Reg(0X04, CS_GYRO); // Y_LSB
    BMI088_GYROData[3] = BMI088_Read_Reg(0X05, CS_GYRO); // Y_MSB
    BMI088_GYROData[4] = BMI088_Read_Reg(0X06, CS_GYRO); // Z_LSB
    BMI088_GYROData[5] = BMI088_Read_Reg(0X07, CS_GYRO); // Z_MSB
}

/**
 * @brief 数据更新触发中断回调，对应UpdataFlag置1
 *
 * @param GPIO_Pin
 */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
    switch (GPIO_Pin)
    {
    case INT1_Pin: // ACC
        BMI088_UpdataACCData();
        break;
    case INT3_Pin: // GYRO
        BMI088_UpdataGYROData();
        break;
    default:
        break;
    }
}

/**
 * @brief 将BMI088读取的寄存器数据装换成六轴实际数据
 * @param
 * @note
 */
void BMI088_ConverData(void)
{
    BMI088.acc_x = (int16_t)((BMI088_ACCData[1] << 8) | BMI088_ACCData[0]) * MPU_ACCE_M_S_2;
    BMI088.acc_y = (int16_t)((BMI088_ACCData[3] << 8) | BMI088_ACCData[2]) * MPU_ACCE_M_S_2;
    BMI088.acc_z = (int16_t)((BMI088_ACCData[5] << 8) | BMI088_ACCData[4]) * MPU_ACCE_M_S_2;

    BMI088.gyro_x = (int16_t)((BMI088_GYROData[1] << 8) | BMI088_GYROData[0]) * MPU_GYRO_TO_DPS ;
    BMI088.gyro_y = (int16_t)((BMI088_GYROData[3] << 8) | BMI088_GYROData[2]) * MPU_GYRO_TO_DPS ;
    BMI088.gyro_z = (int16_t)((BMI088_GYROData[5] << 8) | BMI088_GYROData[4]) * MPU_GYRO_TO_DPS ;
}

void BMI088_GetGYROzError(void)
{
    for (uint32_t i = 0; i < 1000; i++)
    {
        BMI088_ConverData();
		BMI088.x_error += BMI088.gyro_x;
		BMI088.y_error += BMI088.gyro_y;
        BMI088.z_error += BMI088.gyro_z;
		HAL_Delay(1);
    }
    BMI088.x_error /= 1000;
	BMI088.y_error /= 1000;
	BMI088.z_error /= 1000;
}
